#include <Dijkstra.h>


double Dijkstra(int start,int end,double** distance_M_Map,int cnt){
    double* distance_table=new double[cnt];
    for(int i=0;i<cnt;i++){
        distance_table[i]=distance_M_Map[i][end];
    }
    int* minids=new int[cnt];
    for(int i=0;i<cnt;i++){
        minids[i]=0;
    }
    int minids_cnt=0;
    int min_id=end;
    minids[end]=1;
    minids_cnt+=1;
    while(minids_cnt<cnt-1){
        double min=M_M_Map_INF;
        for(int i=0;i<cnt;i++){
            if(minids[i]==1)
                continue;
            if(distance_table[i]<min){
                min=distance_table[i];
                min_id=i;
            }
        }
        minids[min_id]=1;
        minids_cnt=minids_cnt+1;
        for(int i=0;i<cnt;i++){
            if(minids[i]==1){
                continue;
            }
            if(distance_M_Map[i][min_id]+distance_table[min_id]< distance_table[i]){
                distance_table[i]=distance_M_Map[i][min_id]+distance_table[min_id];
                // distance_M_Map[i][end]=distance_M_Map[i][min_id]+distance_table[min_id];
            }
        }
    }
    double result=distance_table[start];
    delete distance_table;
    delete minids;
    return result;
}

trajectory_node_t* Dijkstra_Path(int start,int end,double** distance_M_Map,int cnt){
    double* distance_table=new double[cnt];
    for(int i=0;i<cnt;i++){
        distance_table[i]=distance_M_Map[i][end];
    }
    //minids 指的是已经加入到最短路径集合的节点id,即计算完成到达终点最短路径的节点集合
    int* minids=new int[cnt];
    
    //记录到达终点最短路径的指向
    int* pointers=new int[cnt];
    for(int i=0;i<cnt;i++){
        //越界值表示无法到达
        pointers[i]=cnt+1;
    }
    for(int i=0;i<cnt;i++){
        minids[i]=0;
    }
    int minids_cnt=0;
    int min_id=end;
    minids[end]=1;
    minids_cnt+=1;
    while(minids_cnt<cnt){
        double min=M_M_Map_INF;
        for(int i=0;i<cnt;i++){
            if(minids[i]==1)
                continue;
            if(distance_table[i]<min){
                min=distance_table[i];
                min_id=i;
            }
        }
        minids[min_id]=1;
        if(pointers[min_id]>cnt){
            pointers[min_id]=end;
        }
        minids_cnt=minids_cnt+1;
        for(int i=0;i<cnt;i++){
            if(minids[i]==1){
                continue;
            }
            if(distance_M_Map[i][min_id]+distance_table[min_id]< distance_table[i]){
                distance_table[i]=distance_M_Map[i][min_id]+distance_table[min_id];
                // distance_M_Map[i][end]=distance_M_Map[i][min_id]+distance_table[min_id];
                pointers[i]=min_id;
            }
        }
    }
    delete distance_table;
    delete minids;
    int point=pointers[start];
    trajectory_node_t* t=new trajectory_node_t;
    *t={
        .node_id=start,
        .prev=NULL,
        .next=NULL
    };
    trajectory_node_t* tmp=t;
    while(point!=end){
      trajectory_node_t* t1=new trajectory_node_t;  
      *t1={
          .node_id=point,
          .prev=tmp,
          .next=NULL
      };
      tmp->next=t1;
      tmp->distance_tonext=distance_M_Map[tmp->node_id][t1->node_id];
      tmp=t1;
      point=pointers[point];
    }
    trajectory_node_t* end_t=new trajectory_node_t;
    end_t->node_id=end;
    end_t->next=NULL;
    end_t->prev=tmp;
    tmp->next=end_t;
    tmp->distance_tonext=distance_M_Map[tmp->node_id][end_t->node_id];
    return t;
}

// int main(){
//     M_Map m(6);
//     m.addEdge({1,6,100.0});
//     m.addEdge({1,5,30.0});
//     m.addEdge({1,3,10.0});
//     m.addEdge({2,3,5.0});
//     m.addEdge({3,4,50.0});
//     m.addEdge({4,6,10.0});
//     m.addEdge({5,4,20.0});
//     m.addEdge({5,6,60.0});
//     int start;
//     int end_id;
//     cout<<"start:";
//     cin>>start;
//     cout<<"end:";
//     cin>>end_id;
//     // cout<<"v"<<start<<" to v"<<end_id<<" 's shortest path's length:"<<Dijkstra(start-1,end_id-1,m.getDistanceM_Map(),6)<<endl;
//     trajectory_node_t* t = Dijkstra_Path(start-1,end_id-1,m.getDistanceM_Map(),6);
//     while (t->next!=NULL)
//     {
//         /* code */
//         cout<<"v"<<t->node_id+1<<"--"<<t->distance_tonext<<"-->";
//         t=t->next;
//     }
//     cout<<"v"<<t->node_id+1<<endl;
    
// }